#include "battery/battery.hpp"
#include "agv_msgs/msg/agv_mode.hpp"

CBattery::~CBattery() {
    for (auto& pair : bat_paras_) {
        delete pair.second;  // 释放动态分配的 CdataRecipe 对象
    }
    bat_paras_.clear();
}

void CdataRecipe::init(std::shared_ptr<rclcpp::Node> node, const std::string &prefix)
{
    std::string temp_prefix;
    temp_prefix = prefix + TOPICNAME;
    node->declare_parameter(temp_prefix, "/battery/CMD0001");
    node->get_parameter(temp_prefix, topic_name);
    RCLCPP_INFO(node->get_logger(), "%s: %s", temp_prefix.c_str(), topic_name.c_str());

    temp_prefix = prefix + INDEX_OF_RAW;
    node->declare_parameter(temp_prefix, 0);
    node->get_parameter(temp_prefix, index_of_raw_data_);
    RCLCPP_INFO(node->get_logger(), "%s: %d", temp_prefix.c_str(), index_of_raw_data_);

    temp_prefix = prefix + SCALE;
    node->declare_parameter(temp_prefix, 0.1f);
    node->get_parameter(temp_prefix, scale_);
    RCLCPP_INFO(node->get_logger(), "%s: %f", temp_prefix.c_str(), scale_);

    temp_prefix = prefix + OFFSET;
    node->declare_parameter(temp_prefix, 0.0f);
    node->get_parameter(temp_prefix, offset_);
    RCLCPP_INFO(node->get_logger(), "%s: %f", temp_prefix.c_str(), offset_);

    temp_prefix = prefix + BYTE_LENGTH;
    node->declare_parameter(temp_prefix, 2);
    node->get_parameter(temp_prefix, byte_length_);
    RCLCPP_INFO(node->get_logger(), "%s: %d", temp_prefix.c_str(), byte_length_);

    size_t found = topic_name.find("can"); // 使用find()函数查找字符串位置
    if (found != std::string::npos) { //can
        can_sub_ = node->create_subscription<std_msgs::msg::UInt64>(
            topic_name, 1, std::bind(&CdataRecipe::can_parserCB, this, std::placeholders::_1));
    } else { //485
        rs485_sub_ = node->create_subscription<std_msgs::msg::UInt16MultiArray>(
            topic_name, 1, std::bind(&CdataRecipe::rs485_parserCB, this, std::placeholders::_1));
    }
    
    RCLCPP_INFO(node->get_logger(), "[init]subscribe from %s OK", topic_name.c_str());
}

void CdataRecipe::rs485_parserCB(const std_msgs::msg::UInt16MultiArray::SharedPtr msg) 
{
    product_data_ = parser(*msg);
    active_.Update();
}

void CdataRecipe::can_parserCB(const std_msgs::msg::UInt64::SharedPtr msg) 
{
    product_data_ = parser(*msg);
    active_.Update();
}

void CBattery::batteryLowcheck(void)
{
    if (battery_info_.charge_flag == 1) {
        event_manager_.clearEvent();
        return;
    }

    if (battery_info_.numeric_bat < too_low_limit_) {
        event_manager_.updateEvent(agv_msgs::msg::AGVEvent::BATTERY_TOO_LOW_EVENT,
                                  agv_msgs::msg::AGVEvent::STOP,
                                  agv_msgs::msg::AGVEvent::ALARM,
                                  "[alarm!] battery percent is too low,vehicle need stop,please manual driving to charge",
                                  battery_info_.numeric_bat,
                                  too_low_limit_);
        if (event_manager_.checkEvent(agv_msgs::msg::AGVEvent::BATTERY_LOW_EVENT)) {
            event_manager_.removeEvent(agv_msgs::msg::AGVEvent::BATTERY_LOW_EVENT);
        }
    }
    else if (battery_info_.numeric_bat < low_limit_) {
        event_manager_.updateEvent(agv_msgs::msg::AGVEvent::BATTERY_LOW_EVENT,
                                  agv_msgs::msg::AGVEvent::NONE,
                                  agv_msgs::msg::AGVEvent::ALARM,
                                  "[warning] battery percent is low power,vehicle need charge",
                                  battery_info_.numeric_bat,
                                  low_limit_);
        if (event_manager_.checkEvent(agv_msgs::msg::AGVEvent::BATTERY_TOO_LOW_EVENT)) {
            event_manager_.removeEvent(agv_msgs::msg::AGVEvent::BATTERY_TOO_LOW_EVENT);
        }
    }
    else {
        event_manager_.clearEvent();
    }
}

void CBattery::mode_Callback(const agv_msgs::msg::AGVMode::SharedPtr msg)
{
    agv_mode_.mode = msg->mode;
}

void CBattery::rawDataTimeoutCheck(void)
{
    if (timeout()) {
        event_manager_.updateEvent(agv_msgs::msg::AGVEvent::BATTERY_ERROR_EVENT,
                                  agv_msgs::msg::AGVEvent::NONE,
                                  agv_msgs::msg::AGVEvent::WARNING,
                                  "raw data receive no msg over 10s",
                                  0,
                                  0);
    }
    else if (event_manager_.checkEvent(agv_msgs::msg::AGVEvent::BATTERY_ERROR_EVENT)) {
        event_manager_.removeEvent(agv_msgs::msg::AGVEvent::BATTERY_ERROR_EVENT);
    }
}

bool CBattery::TimeON(bool start, unsigned long duration_ms)
{
    static auto last_time = std::chrono::steady_clock::now();
    static std::chrono::milliseconds accumulated_time(0);
    
    auto now = std::chrono::steady_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_time);
    last_time = now;
    
    if (start) {
        accumulated_time += elapsed;
    } else {
        accumulated_time = std::chrono::milliseconds(0);
    }
    
    return accumulated_time.count() > duration_ms;
}

bool CBattery::haveData(std::string str)
{
    auto iter = bat_paras_.find(str);
    return iter != bat_paras_.end();
}

bool CBattery::batteryInfo_update(void)
{
    bool ret = false;
    float voltage, current;
    uint16_t percent;

    if (isNew()) {
        voltage = haveData(VOLTAGE) ? bat_paras_[VOLTAGE]->product_data_ : 0.0f;
        current = haveData(CURRENT) ? bat_paras_[CURRENT]->product_data_ : 0.0f;
        percent = haveData(PERCENT) ? bat_paras_[PERCENT]->product_data_ : 0;

        if (voltage > 12.0 && voltage < 240.0 && 
            current > -2000.0 && current < 1000.0 && 
            percent >= 0 && percent <= 100) {
            
            battery_info_.voltage = voltage;
            battery_info_.current = current;
            battery_info_.numeric_bat = percent;
            battery_info_.temperature = haveData(TEMPERATURE) ? bat_paras_[TEMPERATURE]->product_data_ : 0.0f;
            battery_info_.cycle_counts = haveData(CYCLE_COUNTS) ? bat_paras_[CYCLE_COUNTS]->product_data_ : 0;
            battery_info_.health = haveData(HEALTH) ? bat_paras_[HEALTH]->product_data_ : -1;
            battery_info_.bms_state = haveData(BMS_STATE) ? bat_paras_[BMS_STATE]->product_data_ : 0;
            battery_info_.error_code = haveData(ERROR_CODE) ? bat_paras_[ERROR_CODE]->product_data_ : 0;
            battery_info_.total_charge_ah = haveData(TOTAL_CHARGE_AH) ? bat_paras_[TOTAL_CHARGE_AH]->product_data_ : 0;
            battery_info_.total_charge_hour = haveData(TOTAL_CHARGE_HOUR) ? bat_paras_[TOTAL_CHARGE_HOUR]->product_data_ : 0;
            battery_info_.total_run_hour = haveData(TOTAL_RUN_HOUR) ? bat_paras_[TOTAL_RUN_HOUR]->product_data_ : 0;
            battery_info_.nominal_capacity = haveData(NOMINAL_CAPACITY) ? bat_paras_[NOMINAL_CAPACITY]->product_data_ : 0;
            battery_info_.field_valid = field_valid_;
            battery_info_.cell_type = cell_type_;
            battery_info_.charge_flag = TimeON(battery_info_.current > charge_current_);
            
            batteryLowcheck();
            
            RCLCPP_DEBUG(node_->get_logger(), "[battery data:] {vol:%f; cur:%f; per:%d}", 
                        voltage, current, percent);
            ret = true;
        }
        else {
            RCLCPP_ERROR(node_->get_logger(), "[thread_fun]error data vol:%f cur:%f per:%d", 
                        voltage, current, percent);
        }
    }

    if (!ignore_) {
        rawDataTimeoutCheck();
    }
    
    return ret;
}

bool CBattery::createRecipe(std::string str)
{
    std::string headStr(HEAD_NAME);
    std::string temp = headStr + "/" + str;
    
    if (node_->has_parameter(temp)) {
        RCLCPP_DEBUG_STREAM(node_->get_logger(), "found: " << temp);
        CdataRecipe* pdataRecipe = new CdataRecipe();
        pdataRecipe->init(node_, temp);
        bat_paras_[str] = pdataRecipe;
        return true;
    } else {
        RCLCPP_DEBUG_STREAM(node_->get_logger(), "not found:" << temp);
        return false;
    }
}

void CBattery::init(std::shared_ptr<rclcpp::Node> node)
{
    node_ = node;
    
    node_->declare_parameter("/battery/needAlarm_limit", 20);
    node_->get_parameter("/battery/needAlarm_limit", low_limit_);
    RCLCPP_INFO(node_->get_logger(), "/battery/needAlarm_limit: %d", low_limit_);

    node_->declare_parameter("/battery/needStop_limit", 5);
    node_->get_parameter("/battery/needStop_limit", too_low_limit_);
    
    if (too_low_limit_ > low_limit_) {
        RCLCPP_ERROR(node_->get_logger(), "too_low_limit_ can't be less than low_limit_");
        too_low_limit_ = -1;
    } else {
        RCLCPP_INFO(node_->get_logger(), "/battery/needStop_limit: %d", too_low_limit_);
    }

    node_->declare_parameter("/battery/ignoreNodata", false);
    node_->get_parameter("/battery/ignoreNodata", ignore_);
    
    node_->declare_parameter("/battery/timeout", TIMEOUT_S);
    node_->get_parameter("/battery/timeout", timeout_);
    
    node_->declare_parameter("/battery/cell_type", "unknown");
    node_->get_parameter("/battery/cell_type", cell_type_);
    
    node_->declare_parameter("/battery/chargeCurrent", CHARGE_CURRENT_DEFAULT);
    node_->get_parameter("/battery/chargeCurrent", charge_current_);
    
    // 获取电池消息topic名称
    if (createRecipe(VOLTAGE)) field_valid_ = SET_BIT(field_valid_, agv_msgs::msg::BatInfo::VOLTAGE_BIT);
    if (createRecipe(CURRENT)) field_valid_ = SET_BIT(field_valid_, agv_msgs::msg::BatInfo::CURRENT_BIT);
    if (createRecipe(PERCENT)) field_valid_ = SET_BIT(field_valid_, agv_msgs::msg::BatInfo::NUMERIC_BAT_BIT);
    if (createRecipe(TEMPERATURE)) field_valid_ = SET_BIT(field_valid_, agv_msgs::msg::BatInfo::TEMPERATURE_BIT);
    if (createRecipe(CYCLE_COUNTS)) field_valid_ = SET_BIT(field_valid_, agv_msgs::msg::BatInfo::CYCLE_COUNTS_BIT);
    if (createRecipe(HEALTH)) field_valid_ = SET_BIT(field_valid_, agv_msgs::msg::BatInfo::HEALTH_BIT);
    if (createRecipe(BMS_STATE)) field_valid_ = SET_BIT(field_valid_, agv_msgs::msg::BatInfo::BMS_STATE_BIT);
    if (createRecipe(ERROR_CODE)) field_valid_ = SET_BIT(field_valid_, agv_msgs::msg::BatInfo::ERROR_CODE_BIT);
    if (createRecipe(TOTAL_CHARGE_AH)) field_valid_ = SET_BIT(field_valid_, agv_msgs::msg::BatInfo::TOTAL_CHARGE_AH_BIT);
    if (createRecipe(TOTAL_CHARGE_HOUR)) field_valid_ = SET_BIT(field_valid_, agv_msgs::msg::BatInfo::TOTAL_CHARGE_HOUR_BIT);
    if (createRecipe(TOTAL_RUN_HOUR)) field_valid_ = SET_BIT(field_valid_, agv_msgs::msg::BatInfo::TOTAL_RUN_HOUR_BIT);
    if (createRecipe(NOMINAL_CAPACITY)) field_valid_ = SET_BIT(field_valid_, agv_msgs::msg::BatInfo::NOMINAL_CAPACITY_BIT);
    
    RCLCPP_INFO(node_->get_logger(), "field_valid_: %d", field_valid_);

    mode_sub_ = node_->create_subscription<agv_msgs::msg::AGVMode>(
        "/agvmode", 2, std::bind(&CBattery::mode_Callback, this, std::placeholders::_1));
    
    RCLCPP_INFO(node_->get_logger(), "[init]publish batteryInfo OK");

    event_manager_.init(node_);
}